using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using Microsoft.Xna.Framework;

namespace Oops.Xna.Framework.Physics
{
	/// <summary>
	/// Represents one contact point between two <see cref="CollisionShape"/> instances
	/// in a collision manifold.
	/// </summary>
	public class CollisionPoint
	{
		private static ObjectPool<CollisionPoint> pool = new ObjectPool<CollisionPoint>(128);
#if DEBUG
		/// <summary>
		/// Gets or sets the position of the <see cref="CollisionPoint"/> in world-coordinates.
		/// </summary>
		public Vector3 Position;
#endif
		/// <summary>
		/// Gets or sets the relative position to the <see cref="CollisionPoint"/> 
		/// of the first <see cref="CollisionShape"/> in the associated <see cref="Collision"/>.
		/// </summary>
		public Vector3 RelativePosition0;

		/// <summary>
		/// Gets or sets the relative position to the <see cref="CollisionPoint"/> 
		/// of the second <see cref="CollisionShape"/> in the associated <see cref="Collision"/>.
		/// </summary>
		public Vector3 RelativePosition1;

		/// <summary>
		/// Gets or sets the penetration depth of the <see cref="CollisionPoint"/>.
		/// </summary>
		public float Depth;

		internal float velocityChange; // This will be the reciprocal during usage.
		internal float desiredVelocityChange;

		/// <summary>
		/// Initialize a new instance of the <see cref="CollisionPoint"/> class.
		/// </summary>
		public CollisionPoint() { }

		/// <summary>
		/// Gets a <see cref="CollisionPoint" /> instance from the pool.
		/// </summary>
		/// <returns>
		/// Returns a fresh <see cref="CollisionPoint"/> instance.
		/// </returns>
		internal static CollisionPoint Retrieve()
		{
			// Get an unused collision point from the pool.
			CollisionPoint point = CollisionPoint.pool.Retrieve();

			// Initialize other data to defaults.
			point.velocityChange = 0.0f;
			point.desiredVelocityChange = 0.0f;
			
			// Return the collision point.
			return point;
		}

		/// <summary>
		/// Returns a <see cref="CollisionPoint"/> instance to the pool.
		/// </summary>
		/// <param name="point">The <see cref="CollisionPoint"/> to recycle.</param>
		internal static void Recycle(CollisionPoint point)
		{
			// Put the collision point back in the pool for later use.
			CollisionPoint.pool.Recycle(point);
		}
	}

	/// <summary>
	/// Represents a collection of <see cref="CollisionPoint"/> instances.
	/// </summary>
	public class CollisionPointCollection : Collection<CollisionPoint>
	{
		private static ObjectPool<CollisionPointCollection> pool = new ObjectPool<CollisionPointCollection>(8);

		/// <summary>
		/// Initializes a new instance of the <see cref="CollisionPointCollection"/> class.
		/// </summary>
		public CollisionPointCollection() { }

		/// <summary>
		/// Gets a <see cref="CollisionPointCollection" /> instance from the pool.
		/// </summary>
		/// <returns>
		/// Returns a fresh <see cref="CollisionPointCollection"/> instance.
		/// </returns>
		internal static CollisionPointCollection Retrieve()
		{
			// Return an unused collision point collection from the pool.
			return CollisionPointCollection.pool.Retrieve();
		}

		/// <summary>
		/// Returns a <see cref="CollisionPointCollection"/> instance to the pool.
		/// </summary>
		/// <param name="collisionPoints">The <see cref="CollisionPointCollection"/> to recycle.</param>
		internal static void Recycle(CollisionPointCollection collisionPoints)
		{
			// Uninitialize.
			collisionPoints.Clear();

			// Put the collision point collection back in the pool for later use.
			CollisionPointCollection.pool.Recycle(collisionPoints);
		}
	}

	// TODO : Derive from Constraint???  They're close in functionality and usage.
	/// <summary>
	/// Represents a collision manifold between two <see cref="CollisionShape"/> instances.
	/// </summary>
	public class Collision
	{
		private static ObjectPool<Collision> pool = new ObjectPool<Collision>(128);

		/// <summary>
		/// Gets or sets the first <see cref="CollisionShape"/> involved in the collision.
		/// </summary>
		public CollisionShape Shape0;

		/// <summary>
		/// Gets or sets the second <see cref="CollisionShape"/> involved in the collision.
		/// </summary>
		public CollisionShape Shape1;

		/// <summary>
		/// Gets or sets the direction to the first (zero) <see cref="CollisionShape"/> from
		/// the second <see cref="CollisionShape"/>.
		/// </summary>
		public Vector3 Normal;

		private List<CollisionPoint> points = new List<CollisionPoint>(12);

		internal float staticFriction;
		internal float dynamicFriction;
		internal float restitution;

		internal bool satisfied;
	            
		/// <summary>
		/// Initializes a new instance of the <see cref="Collision"/> class.
		/// </summary>
		public Collision() { }

		/// <summary>
		/// Gets all the <see cref="CollisionPoint"/> instances involved in the collision.
		/// </summary>
		public List<CollisionPoint> Points
		{
			get { return this.points; }
		}

		/// <summary>
		/// Called before an attempt to resolve the <see cref="Collision"/> is made.
		/// </summary>
		/// <param name="elapsed">The simulation step's total elapsed time.</param>
		internal void PreStep(float elapsed)
		{
			// Reset satisfied flag.
			this.satisfied = false;

			// Get the rigid bodies involved in this collision for easier access.
			RigidBody rigidBody0 = this.Shape0.CollisionGroup.RigidBody;
			RigidBody rigidBody1 = this.Shape1.CollisionGroup.RigidBody;

			// TODO : Make parameter.
			// Relax the resolution over a few frames.
			const int relaxationCount = 12;
			float inverseElapsed = 1.0f / MathHelper.Max(relaxationCount * elapsed, MathExtension.Epsilon);

			// Iterate on all the contact points of this collision.
			for (int i = 0; i < this.Points.Count; i++)
			{
				// Get the current contact point.
				CollisionPoint point = this.Points[i];

				// Determine the desired velocity change of this collision point.
				point.desiredVelocityChange = point.Depth * inverseElapsed;
								
				// Limit the desired velocity change in the direction of the collision normal.
				point.desiredVelocityChange = MathHelper.Clamp(point.desiredVelocityChange, 0.0f, 2.5f);

				// Determine the relative velocity change magnitude of the first rigid body at the collision position.
				point.velocityChange = 0.0f;
				if (rigidBody0 != null && (rigidBody0.Flags & RigidBodyFlags.Static) == RigidBodyFlags.None)
				{
#if INLINE
					float x = point.RelativePosition0.Y * this.Normal.Z - point.RelativePosition0.Z * this.Normal.Y;
					float y = point.RelativePosition0.Z * this.Normal.X - point.RelativePosition0.X * this.Normal.Z;
					float z = point.RelativePosition0.X * this.Normal.Y - point.RelativePosition0.Y * this.Normal.X;

					float tx = x * rigidBody0.inverseInertiaTensorWorld.M11 + y * rigidBody0.inverseInertiaTensorWorld.M21 + z * rigidBody0.inverseInertiaTensorWorld.M31;
					float ty = x * rigidBody0.inverseInertiaTensorWorld.M12 + y * rigidBody0.inverseInertiaTensorWorld.M22 + z * rigidBody0.inverseInertiaTensorWorld.M32;
					float tz = x * rigidBody0.inverseInertiaTensorWorld.M13 + y * rigidBody0.inverseInertiaTensorWorld.M23 + z * rigidBody0.inverseInertiaTensorWorld.M33;

					x = ty * point.RelativePosition0.Z - tz * point.RelativePosition0.Y;
					y = tz * point.RelativePosition0.X - tx * point.RelativePosition0.Z;
					z = tx * point.RelativePosition0.Y - ty * point.RelativePosition0.X;

					point.velocityChange = rigidBody0.inverseMass + (this.Normal.X * x + this.Normal.Y * y + this.Normal.Z * z);
#elif REFERENCE
					Vector3 temp;
					Vector3.Cross(ref point.RelativePosition0, ref this.Normal, out temp);
					Vector3.TransformNormal(ref temp, ref rigidBody0.inverseInertiaTensorWorld, out temp);
					Vector3.Cross(ref temp, ref point.RelativePosition0, out temp);
					point.velocityChange = rigidBody0.inverseMass + Vector3Extension.Dot(ref this.Normal, ref temp);
#else
					point.velocityChange = rigidBody0.inverseMass + Vector3.Dot(this.Normal, Vector3.Cross(Vector3.TransformNormal(Vector3.Cross(point.RelativePosition0, this.Normal), rigidBody0.inverseInertiaTensorWorld), point.RelativePosition0));
#endif
				}

				// Determine the relative velocity change magnitude of the first second body at the collision position.
				// Apply it the first rigid body's velocity change.
				if (rigidBody1 != null && (rigidBody1.Flags & RigidBodyFlags.Static) == RigidBodyFlags.None)
				{
#if INLINE
					float x = point.RelativePosition1.Y * this.Normal.Z - point.RelativePosition1.Z * this.Normal.Y;
					float y = point.RelativePosition1.Z * this.Normal.X - point.RelativePosition1.X * this.Normal.Z;
					float z = point.RelativePosition1.X * this.Normal.Y - point.RelativePosition1.Y * this.Normal.X;

					float tx = x * rigidBody1.inverseInertiaTensorWorld.M11 + y * rigidBody1.inverseInertiaTensorWorld.M21 + z * rigidBody1.inverseInertiaTensorWorld.M31;
					float ty = x * rigidBody1.inverseInertiaTensorWorld.M12 + y * rigidBody1.inverseInertiaTensorWorld.M22 + z * rigidBody1.inverseInertiaTensorWorld.M32;
					float tz = x * rigidBody1.inverseInertiaTensorWorld.M13 + y * rigidBody1.inverseInertiaTensorWorld.M23 + z * rigidBody1.inverseInertiaTensorWorld.M33;

					x = ty * point.RelativePosition1.Z - tz * point.RelativePosition1.Y;
					y = tz * point.RelativePosition1.X - tx * point.RelativePosition1.Z;
					z = tx * point.RelativePosition1.Y - ty * point.RelativePosition1.X;

					point.velocityChange = point.velocityChange + rigidBody1.inverseMass + (this.Normal.X * x + this.Normal.Y * y + this.Normal.Z * z);
#elif REFERENCE
					Vector3 temp;
					Vector3.Cross(ref point.RelativePosition1, ref this.Normal, out temp);
					Vector3.TransformNormal(ref temp, ref rigidBody1.inverseInertiaTensorWorld, out temp);
					Vector3.Cross(ref temp, ref point.RelativePosition1, out temp);
					point.velocityChange = point.velocityChange + rigidBody1.inverseMass + Vector3Extension.Dot(ref this.Normal, ref temp);
#else
					point.velocityChange += rigidBody1.inverseMass + Vector3.Dot(this.Normal, Vector3.Cross(Vector3.TransformNormal(Vector3.Cross(point.RelativePosition1, this.Normal), rigidBody1.inverseInertiaTensorWorld), point.RelativePosition1));
#endif
				}

				// Make sure the relative velocity change does not cause division issues when determining the 
				// magnitude of the impulse needed to resolve a collision along the collision normal.
				if (point.velocityChange < MathExtension.Epsilon)
				{
					point.velocityChange = MathExtension.Epsilon;
				}

				// Store the reciprocal.
				point.velocityChange = 1.0f / point.velocityChange;
			}
		}

		/// <summary>
		/// Attempts to solve the <see cref="Collision"/>.
		/// </summary>
		/// <param name="elapsed">The simulation step's total elapsed time.</param>
		/// <returns><b>true</b>, if the <see cref="Collision"/> was enforced. <b>false</b>, otherwise.</returns>
		internal bool Solve(float elapsed)
		{
			// Get the rigid bodies involved in this collision for easier access.
			RigidBody rigidBody0 = this.Shape0.CollisionGroup.RigidBody;
			RigidBody rigidBody1 = this.Shape1.CollisionGroup.RigidBody;

			// Assume the collision is satisfied at the start.
			this.satisfied = true;

			// Iterate through all the collision's contact points.
			bool result = false;
			for (int i = 0; i < this.Points.Count; i++)
			{
				// Get the current contact point.
				CollisionPoint point = this.Points[i];
#if INLINE
				// Get the relative velocity to the collision point of the first rigid body.
				Vector3 temp = Vector3.Zero;
				if (rigidBody0 != null)
				{
					temp.X = rigidBody0.Velocity.X + (rigidBody0.Rotation.Y * point.RelativePosition0.Z - rigidBody0.Rotation.Z * point.RelativePosition0.Y);
					temp.Y = rigidBody0.Velocity.Y + (rigidBody0.Rotation.Z * point.RelativePosition0.X - rigidBody0.Rotation.X * point.RelativePosition0.Z);
					temp.Z = rigidBody0.Velocity.Z + (rigidBody0.Rotation.X * point.RelativePosition0.Y - rigidBody0.Rotation.Y * point.RelativePosition0.X);
				}

				// Get the relative velocity to the collision point of the second rigid body.  Apply it to the first.
				if (rigidBody1 != null)
				{
					temp.X = temp.X - (rigidBody1.Velocity.X + (rigidBody1.Rotation.Y * point.RelativePosition1.Z - rigidBody1.Rotation.Z * point.RelativePosition1.Y));
					temp.Y = temp.Y - (rigidBody1.Velocity.Y + (rigidBody1.Rotation.Z * point.RelativePosition1.X - rigidBody1.Rotation.X * point.RelativePosition1.Z));
					temp.Z = temp.Z - (rigidBody1.Velocity.Z + (rigidBody1.Rotation.X * point.RelativePosition1.Y - rigidBody1.Rotation.Y * point.RelativePosition1.X));
				}

				// Determine the velocity magnitude in the direction of the collision normal.
				float normalVelocity = temp.X * this.Normal.X + temp.Y * this.Normal.Y + temp.Z * this.Normal.Z;
#elif REFERENCE
				// Get the relative velocity to the collision point of the first rigid body.
				Vector3 temp0 = Vector3.Zero;
				if (rigidBody0 != null)
				{
					Vector3.Cross(ref rigidBody0.Rotation, ref point.RelativePosition0, out temp0);
					Vector3.Add(ref temp0, ref rigidBody0.Velocity, out temp0);
				}

				// Get the relative velocity to the collision point of the second rigid body.  Apply it to the first.
				if (rigidBody1 != null)
				{
					Vector3 temp1;
					Vector3.Cross(ref rigidBody1.Rotation, ref point.RelativePosition1, out temp1);
					Vector3.Add(ref temp1, ref rigidBody1.Velocity, out temp1);

					Vector3.Subtract(ref temp0, ref temp1, out temp0);
				}

				// Determine the velocity magnitude in the direction of the collision normal.
				float normalVelocity;
				Vector3.Dot(ref temp0, ref this.Normal, out normalVelocity);
#else
				// Get the relative velocity to the collision point of the first rigid body.
				Vector3 temp = Vector3.Zero;
				if (rigidBody0 != null)
				{
					temp = rigidBody0.Velocity + Vector3.Cross(rigidBody0.Rotation, point.RelativePosition0);
				}

				// Get the relative velocity to the collision point of the second rigid body.  Apply it to the first.
				if (rigidBody1 != null)
				{
					temp -= rigidBody1.Velocity + Vector3.Cross(rigidBody1.Rotation, point.RelativePosition1);
				}

				// Determine the velocity magnitude in the direction of the collision normal.
				float normalVelocity = Vector3.Dot(temp, this.Normal);
#endif
				// Check if the rigid body is already heading towards resolving this collision point.
				if (normalVelocity > point.desiredVelocityChange)
				{
					continue;
				}

				// Apply the restitution coefficient.
				float restitutedNormalVelocity = -this.restitution * normalVelocity;
				if (restitutedNormalVelocity < 0.0009f)
				{
					restitutedNormalVelocity = point.desiredVelocityChange;
				}

				// Check threshold for minimum required velocity change. 
				float deltaVelocity = restitutedNormalVelocity - normalVelocity;
				if (deltaVelocity <= 0.0009f)
				{
					continue;
				}

				// Determine the magnitude of the impulse needed to resolve this collision point along
				// the collision normal.  The value stored in velocity change is the reciprocal.  This
				// is a division operation.
				float normalImpulse = deltaVelocity * point.velocityChange;

				// Determine the vector impulse.
#if INLINE
#if XBOX
				Vector3 impulse = Vector3.Zero;
#else
				Vector3 impulse;
#endif
				impulse.X = this.Normal.X * normalImpulse;
				impulse.Y = this.Normal.Y * normalImpulse;
				impulse.Z = this.Normal.Z * normalImpulse;
#elif REFERENCE
#if XBOX
				Vector3 impulse = Vector3.Zero;
#else
				Vector3 impulse;
#endif
				Vector3.Multiply(ref this.Normal, normalImpulse, out impulse);
#else
				Vector3 impulse = this.Normal * normalImpulse;
#endif
				// Apply the impulse to the rigid bodies involved in this collision.
				if (rigidBody0 != null && (rigidBody0.Flags & RigidBodyFlags.NoResponse) == RigidBodyFlags.None)
				{
					rigidBody0.ApplyOffsetForceImpulse(ref impulse, ref point.RelativePosition0);
				}
				if (rigidBody1 != null && (rigidBody1.Flags & RigidBodyFlags.NoResponse) == RigidBodyFlags.None)
				{
					rigidBody1.ApplyReverseOffsetForceImpulse(ref impulse, ref point.RelativePosition1);
				}

				// This collision need to be resolved based on the current collision point.
				result = true;

				// Frictionless collisions don't require the following processing.
				if (this.staticFriction == 0.0f && this.dynamicFriction == 0.0f)
				{
					continue;
				}

				// Get the relative velocity to the collision point of the first rigid body again.  It has
				// changed because of the applied impulse above.
				Vector3 relativeVelocity = Vector3.Zero;
				if (rigidBody0 != null)
				{
#if INLINE
					relativeVelocity.X = rigidBody0.Velocity.X + (rigidBody0.Rotation.Y * point.RelativePosition0.Z - rigidBody0.Rotation.Z * point.RelativePosition0.Y);
					relativeVelocity.Y = rigidBody0.Velocity.Y + (rigidBody0.Rotation.Z * point.RelativePosition0.X - rigidBody0.Rotation.X * point.RelativePosition0.Z);
					relativeVelocity.Z = rigidBody0.Velocity.Z + (rigidBody0.Rotation.X * point.RelativePosition0.Y - rigidBody0.Rotation.Y * point.RelativePosition0.X);
#elif REFERENCE
					Vector3.Cross(ref rigidBody0.Rotation, ref point.RelativePosition0, out relativeVelocity);
					Vector3.Add(ref relativeVelocity, ref rigidBody0.Velocity, out relativeVelocity);
#else
					relativeVelocity = rigidBody0.Velocity + Vector3.Cross(rigidBody0.Rotation, point.RelativePosition0);
#endif
				}

				// Get the relative velocity to the collision point of the secong rigid body again.  It has
				// changed because of the applied impulse above.  Apply it the first rigid body's relative velocity.
				if (rigidBody1 != null)
				{
#if INLINE
					relativeVelocity.X -= rigidBody1.Velocity.X + (rigidBody1.Rotation.Y * point.RelativePosition1.Z - rigidBody1.Rotation.Z * point.RelativePosition1.Y);
					relativeVelocity.Y -= rigidBody1.Velocity.Y + (rigidBody1.Rotation.Z * point.RelativePosition1.X - rigidBody1.Rotation.X * point.RelativePosition1.Z);
					relativeVelocity.Z -= rigidBody1.Velocity.Z + (rigidBody1.Rotation.X * point.RelativePosition1.Y - rigidBody1.Rotation.Y * point.RelativePosition1.X);
#elif REFERENCE
					Vector3 temp;
					Vector3.Cross(ref rigidBody1.Rotation, ref point.RelativePosition1, out temp);
					Vector3.Add(ref temp, ref rigidBody1.Velocity, out temp);
					Vector3.Subtract(ref relativeVelocity, ref temp, out relativeVelocity);
#else
					relativeVelocity -= rigidBody1.Velocity + Vector3.Cross(rigidBody1.Rotation, point.RelativePosition1);
#endif
				}

				// Determine the velocity at the collision position perpendicular to the collision normal.  This
				// gives the surface velocity needed to apply friction.
#if INLINE
#if XBOX
				Vector3 perpendicularVelocity = Vector3.Zero;
#else
				Vector3 perpendicularVelocity;
#endif
				float dot = relativeVelocity.X * this.Normal.X + relativeVelocity.Y * this.Normal.Y + relativeVelocity.Z * this.Normal.Z;
				perpendicularVelocity.X = relativeVelocity.X - this.Normal.X * dot;
				perpendicularVelocity.Y = relativeVelocity.Y - this.Normal.Y * dot;
				perpendicularVelocity.Z = relativeVelocity.Z - this.Normal.Z * dot;
#elif REFERENCE
#if XBOX
				Vector3 perpendicularVelocity = Vector3.Zero;
#else
				Vector3 perpendicularVelocity;
#endif
				float dot;
				Vector3.Dot(ref relativeVelocity, ref this.Normal, out dot);
				Vector3.Multiply(ref this.Normal, dot, out perpendicularVelocity);
				Vector3.Subtract(ref relativeVelocity, ref perpendicularVelocity, out perpendicularVelocity);
#else
				Vector3 perpendicularVelocity = relativeVelocity - (this.Normal * Vector3.Dot(relativeVelocity, this.Normal));
#endif
				float perpendicularSpeed = perpendicularVelocity.Length();
				if (perpendicularSpeed > 0.0009f)
				{
					// Get the direction at the collision position to apply friction.
					Vector3 frictionDirection = perpendicularVelocity / -perpendicularSpeed;

					// Determine the magnitude of an impulse necessary ro remove all relative velocity at the collision position of
					// the first rigid body.  This takes into account the rigid body's mass properties of course.
					float denominator = 0.0f;
					if (rigidBody0 != null && (rigidBody0.Flags & RigidBodyFlags.Static) == RigidBodyFlags.None)
					{
#if INLINE
						float x = point.RelativePosition0.Y * frictionDirection.Z - point.RelativePosition0.Z * frictionDirection.Y;
						float y = point.RelativePosition0.Z * frictionDirection.X - point.RelativePosition0.X * frictionDirection.Z;
						float z = point.RelativePosition0.X * frictionDirection.Y - point.RelativePosition0.Y * frictionDirection.X;

						float tx = x * rigidBody0.inverseInertiaTensorWorld.M11 + y * rigidBody0.inverseInertiaTensorWorld.M21 + z * rigidBody0.inverseInertiaTensorWorld.M31;
						float ty = x * rigidBody0.inverseInertiaTensorWorld.M12 + y * rigidBody0.inverseInertiaTensorWorld.M22 + z * rigidBody0.inverseInertiaTensorWorld.M32;
						float tz = x * rigidBody0.inverseInertiaTensorWorld.M13 + y * rigidBody0.inverseInertiaTensorWorld.M23 + z * rigidBody0.inverseInertiaTensorWorld.M33;

						x = ty * point.RelativePosition0.Z - tz * point.RelativePosition0.Y;
						y = tz * point.RelativePosition0.X - tx * point.RelativePosition0.Z;
						z = tx * point.RelativePosition0.Y - ty * point.RelativePosition0.X;

						denominator = rigidBody0.inverseMass + (frictionDirection.X * x + frictionDirection.Y * y + frictionDirection.Z * z);
#elif REFERENCE
						Vector3 temp;
						Vector3.Cross(ref point.RelativePosition0, ref frictionDirection, out temp);
						Vector3.TransformNormal(ref temp, ref rigidBody0.inverseInertiaTensorWorld, out temp);
						Vector3.Cross(ref temp, ref point.RelativePosition0, out temp);
						denominator = rigidBody0.inverseMass + Vector3Extension.Dot(ref frictionDirection, ref temp);
#else
						denominator = rigidBody0.inverseMass + Vector3.Dot(frictionDirection, Vector3.Cross(Vector3.TransformNormal(Vector3.Cross(point.RelativePosition0, frictionDirection), rigidBody0.inverseInertiaTensorWorld), point.RelativePosition0));
#endif
					}

					// Determine the magnitude of an impulse necessary ro remove all relative velocity at the collision position of
					// the second rigid body.  Apply it to the first rigid body's magnitude.
					if (rigidBody1 != null && (rigidBody1.Flags & RigidBodyFlags.Static) == RigidBodyFlags.None)
					{
#if INLINE
						float x = point.RelativePosition1.Y * frictionDirection.Z - point.RelativePosition1.Z * frictionDirection.Y;
						float y = point.RelativePosition1.Z * frictionDirection.X - point.RelativePosition1.X * frictionDirection.Z;
						float z = point.RelativePosition1.X * frictionDirection.Y - point.RelativePosition1.Y * frictionDirection.X;

						float tx = x * rigidBody1.inverseInertiaTensorWorld.M11 + y * rigidBody1.inverseInertiaTensorWorld.M21 + z * rigidBody1.inverseInertiaTensorWorld.M31;
						float ty = x * rigidBody1.inverseInertiaTensorWorld.M12 + y * rigidBody1.inverseInertiaTensorWorld.M22 + z * rigidBody1.inverseInertiaTensorWorld.M32;
						float tz = x * rigidBody1.inverseInertiaTensorWorld.M13 + y * rigidBody1.inverseInertiaTensorWorld.M23 + z * rigidBody1.inverseInertiaTensorWorld.M33;

						x = ty * point.RelativePosition1.Z - tz * point.RelativePosition1.Y;
						y = tz * point.RelativePosition1.X - tx * point.RelativePosition1.Z;
						z = tx * point.RelativePosition1.Y - ty * point.RelativePosition1.X;

						denominator = denominator + rigidBody1.inverseMass + (frictionDirection.X * x + frictionDirection.Y * y + frictionDirection.Z * z);
#elif REFERENCE
						Vector3 temp;
						Vector3.Cross(ref point.RelativePosition1, ref frictionDirection, out temp);
						Vector3.TransformNormal(ref temp, ref rigidBody1.inverseInertiaTensorWorld, out temp);
						Vector3.Cross(ref temp, ref point.RelativePosition1, out temp);
						denominator = denominator + rigidBody1.inverseMass + Vector3Extension.Dot(ref frictionDirection, ref temp);
#else
						denominator += rigidBody1.inverseMass + Vector3.Dot(frictionDirection, Vector3.Cross(Vector3.TransformNormal(Vector3.Cross(point.RelativePosition1, frictionDirection), rigidBody1.inverseInertiaTensorWorld), point.RelativePosition1));
#endif
					}

					if (denominator > MathExtension.Epsilon)
					{
						// Determine the magnitude of the impulse needed to apply friction in the direction
						// of the surface (tangent) of the collision position.
						float frictionImpulse = perpendicularSpeed / denominator;

						// Determine if dynamic friction comes into play.
						if (frictionImpulse >= this.staticFriction * normalImpulse)
						{
							frictionImpulse = this.dynamicFriction * normalImpulse;
						}

						// Determine the vector friction impulse.
#if INLINE
						frictionDirection.X = frictionDirection.X * frictionImpulse;
						frictionDirection.Y = frictionDirection.Y * frictionImpulse;
						frictionDirection.Z = frictionDirection.Z * frictionImpulse;
#elif REFERENCE
						Vector3.Multiply(ref frictionDirection, frictionImpulse, out frictionDirection);
#else
						frictionDirection *= frictionImpulse;
#endif
						// Apply the friction impulse to the rigid bodies involved in this collision.
						if (rigidBody0 != null && (rigidBody0.Flags & RigidBodyFlags.NoResponse) == RigidBodyFlags.None)
						{
							rigidBody0.ApplyOffsetForceImpulse(ref frictionDirection, ref point.RelativePosition0);
						}
						if (rigidBody1 != null && (rigidBody1.Flags & RigidBodyFlags.NoResponse) == RigidBodyFlags.None)
						{
							rigidBody1.ApplyReverseOffsetForceImpulse(ref frictionDirection, ref point.RelativePosition1);
						}
					}
				}
			}

			// The rigid bodies need another go at resolution.
			if (result)
			{
				if (rigidBody0 != null)
				{
					rigidBody0.CollisionGroup.Collisions.Reset();
					rigidBody0.Constraints.Reset();
				}
				if (rigidBody1 != null)
				{
					rigidBody1.CollisionGroup.Collisions.Reset();
					rigidBody1.Constraints.Reset();
				}

				// Keep this collision satisfied.
				this.satisfied = true;
			}

			// Return status of resolution.
			return result;
		}

		/// <summary>
		/// Gets a <see cref="Collision" /> instance from the pool.
		/// </summary>
		/// <returns>
		/// Returns a fresh <see cref="Collision"/> instance.
		/// </returns>
		internal static Collision Retrieve()
		{
			// Get an unused collision from the pool.
			return Collision.pool.Retrieve();
		}

		/// <summary>
		/// Returns a <see cref="Collision"/> instance to the pool.
		/// </summary>
		/// <param name="collision">The <see cref="Collision"/> to recycle.</param>
		internal static void Recycle(Collision collision)
		{
			// Uninitialize.
			collision.Shape0 = null;
			collision.Shape1 = null;

			// Put the collision back in the pool for later use.
			Collision.pool.Recycle(collision);

			// Recycle all associated collision points as well.
			for (int i = 0; i < collision.Points.Count; i++)
			{
				CollisionPoint.Recycle(collision.Points[i]);
			}
			collision.Points.Clear();
		}
	}
}
